#include "trackexecutor.h"

TrackExecutor::TrackExecutor()
{
    _start_drag = false;
    _start_tarck= false;
    _selection = false;

    ros::NodeHandle nh;
    nh.param("HOG",_HOG,false);
    nh.param("FIXEDWINDOW",_FIXEDWINDOW,false);
    nh.param("MULTISCALE",_MULTISCALE,false);
    nh.param("SILENT",_SILENT,false);
    nh.param("LAB",_LAB,false);

    _tracker = new KCFTracker(_HOG,_FIXEDWINDOW,_MULTISCALE,_LAB);
}

void TrackExecutor::OnMouse(int event, int x, int y, int flags, void *userdata)
{
    TrackExecutor* temp = reinterpret_cast<TrackExecutor*>(userdata);
    temp->On_Mouse(event,x,y);
}

void TrackExecutor::On_Mouse(int event, int x, int y)
{
    if(event == EVENT_LBUTTONDOWN)
    {
        _pre_point =  Point(x,y);
        _start_drag = true;
    }

    if(_start_drag)
    {
        _rect.x_min = min(_pre_point.x,x);
        _rect.y_min = min(_pre_point.y,y);
        _rect.x_max = max(_pre_point.x,x);
        _rect.y_max = max(_pre_point.y,y);
        _selection = true;
    }

    if(event == EVENT_LBUTTONUP)
    {
        _start_drag = false;
        _selection = false;
        _start_tarck = true;
        if(_rect.x_max-_rect.x_min>0 && _rect.y_max-_rect.y_min){
//            cout<<rect.x_max-rect.x_min;
            Rect r(_rect.x_min,_rect.y_min,_rect.x_max-_rect.x_min,_rect.y_max-_rect.y_min);
            _tracker.init(r,_frame);
        }
    }
}

void TrackExecutor::RunMain()
{
    string videoFileName = "/home/yan/develop/union_kcf_tracker/DJI_0013.avi";
    auto cap = VideoCapture(videoFileName);
    if(!cap.isOpened())
        return;
    namedWindow("KcfTrackerWindow",WINDOW_NORMAL);
    resizeWindow("KcfTrackerWindow",1080,960);
    setMouseCallback("KcfTrackerWindow", OnMouse, this);

    ros::Rate loop_rate(500);
    while(ros::ok())
    {
        cap >> _frame;
        if(_frame.empty())
        {
            cout<<"over...";
            break;
        }

        if(_selection)
        {
            rectangle(_frame,cvPoint(_rect.x_min,_rect.y_min),cvPoint(_rect.x_max,_rect.y_max),Scalar(255,0,0),2,1);
        }

        if(_start_tarck)
        {
            _result = _tracker.update(_frame);
        }

        if(!_result.empty())
        {
            rectangle(_frame,CvPoint(_result.x,_result.y),CvPoint(_result.x+_result.width,_result.y+_result.height),Scalar(0,0,255),2,1);
        }

        imshow("KcfTrackerWindow",_frame);
        auto k = waitKey(33) & 0xff;
        if(k == 27)
        {
            break;
        }
        loop_rate.sleep();
    }

    cap.release();
    destroyAllWindows();
}


